import numpy as np


class DealData:
    def __init__(self, all_data, lidar_resolution, blind_area, pole_bind_angle):
        self.all_data = all_data
        self.lidar_resolution = lidar_resolution
        self.pole_bind_angle = pole_bind_angle
        self.angle_v = blind_area['angle_v']
        self.distance = blind_area['distance']
        self.multiple = blind_area['multiple']
        self.has_pole = blind_area['has_pole']
        self.position_list = []
        self.mask_list = []
        self.angle_area = []

    def get_position_data(self):
        return self.position_list

    def filter_points(self):
        # 每圈的点数
        num = int(360.0 / self.lidar_resolution)
        # 建立空列表
        xy_angle_list = []
        for m in range(num):
            xy_angle_list.append([])
        data = self.all_data[:, :2]
        distances = np.linalg.norm(data, axis=1)
        mask = (distances <= self.distance) & (distances > 0)
        filtered_points = self.all_data[mask]
        # 计算每条射线的平均点数
        average_num = int(len(filtered_points) / num)
        # 遍历每条射线放入数组
        for i in range(len(filtered_points)):
            xy_abgle = np.arctan2(filtered_points[i][1], filtered_points[i][0])
            xy_abgle = -np.degrees(xy_abgle) + 90
            if xy_abgle < 0:
                xy_abgle += 360
            angle = int(xy_abgle / self.lidar_resolution) % num
            if (360 - xy_abgle) <= (self.lidar_resolution / 2):
                angle = 0
            else:
                if (xy_abgle / self.lidar_resolution) - int(xy_abgle / self.lidar_resolution) > 0.5:
                    angle += 1
            xy_angle_list[angle].append(filtered_points[i])
        return filtered_points, average_num, xy_angle_list

    def get_data_list(self):
        for i in range(len(self.angle_area)):
            min_angle = np.radians(self.angle_area[i][0])
            max_angle = np.radians(self.angle_area[i][1])
            min_position = (np.sin(min_angle), np.cos(min_angle))
            max_position = (np.sin(max_angle), np.cos(max_angle))
            self.position_list.append([min_position, max_position])

    def get_angle_list(self):
        angle_area = []
        for i in range(len(self.mask_list)):
            self.mask_list[i] = self.mask_list[i] * self.lidar_resolution
        # print("self.mask_list", self.mask_list)
        min_angle = self.mask_list[0]
        max_angle = min_angle
        for i in range(1, len(self.mask_list)):
            # 转换为角度（0，360）
            self.mask_list[i] = self.mask_list[i]
            if self.mask_list[i] < self.mask_list[i - 1] + self.angle_v:
                # 更新最大值
                max_angle = self.mask_list[i]
            else:
                # 添加新的数组
                angle_area.append([min_angle, max_angle])
                min_angle = self.mask_list[i]
                max_angle = self.mask_list[i]
        # 添加最后一个数组
        angle_area.append([min_angle, max_angle])
        # print("angle_area", angle_area)
        bound_flag = False
        del_mask = []
        for j in range(len(angle_area)):
            if angle_area[j][0] < self.angle_v:
                bound_flag = True
            if bound_flag and angle_area[j][1] > (360 - self.angle_v):
                angle_area[0][0] = angle_area[j][0] - 360
                del_mask.append(j)
            if angle_area[j][1] - angle_area[j][0] < self.angle_v:
                del_mask.append(j)
        # print("del_mask", del_mask)
        for i in range(len(angle_area)):
            if i in del_mask:
                continue
            else:
                self.angle_area.append(angle_area[i])

    def deal_pole_bind(self):
        if self.pole_bind_angle[0] > self.pole_bind_angle[1]:
            self.pole_bind_angle[0] -= 360
        if len(self.angle_area) == 1:
            # 处理单条盲区
            if self.angle_area[0][0] >= self.pole_bind_angle[1]:
                if self.angle_area[0][0] < self.pole_bind_angle[1] + self.angle_v:
                    self.angle_area[0][0] = self.pole_bind_angle[0]
                else:
                    self.angle_area.append(self.pole_bind_angle)
            else:
                if self.angle_area[0][1] > self.pole_bind_angle[0] - self.angle_v:
                    self.angle_area[0][1] = self.pole_bind_angle[1]
                else:
                    self.angle_area.append(self.pole_bind_angle)
        elif len(self.angle_area) == 0:
            # 处理无盲区
            self.angle_area.append(self.pole_bind_angle)
        else:
            # 处理多条盲区
            for j in range(len(self.angle_area)-1):
                if self.angle_area[j][1] <= self.pole_bind_angle[0] and self.angle_area[j+1][0] >= self.pole_bind_angle[1]:
                    if self.angle_area[j][1] > self.pole_bind_angle[0] - self.angle_v:
                        self.angle_area[j][1] = self.pole_bind_angle[1]
                        if self.angle_area[j+1][0] < self.pole_bind_angle[1] + self.angle_v:
                            self.angle_area[j][1] = self.angle_area[j+1][1]
                            del self.angle_area[j+1]
                    elif self.angle_area[j+1][0] < self.pole_bind_angle[1] + self.angle_v:
                        self.angle_area[j+1][0] = self.pole_bind_angle[0]
                    else:
                        self.angle_area.append(self.pole_bind_angle)
                    break

    def run(self):

        filtered_points, average_num, xy_angle_list = self.filter_points()
        # 筛选出盲区角度序号
        for i in range(len(xy_angle_list)):
            if len(xy_angle_list[i]) > (average_num * self.multiple):
                self.mask_list.append(i)

        # 从小到大排序
        self.mask_list.sort()
        # print(self.mask_list)

        # 获取盲区角度范围
        self.get_angle_list()

        if self.has_pole:
            # 处理路灯杆盲区范围
            self.deal_pole_bind()
        # print(self.angle_area)

        # 获取角度对应的方向向量
        self.get_data_list()
        # print(self.position_list)
        print("雷达盲区划分完成")



